uint8 IDLE = 0 
uint8 BALANCE = 1
uint8 TORQUESERVO = 2
uint8 STATESERVO = 3
uint8 PATHFOLLOW = 4

uint8 mode